#include "qe_log.h"
#include "qe_macros.h"
#include "qe_string.h"
#include "qe_memory.h"
#include "qe_backtrace.h"



QELOG_DOMAIN(QELOG_DOMAIN_BACKTRACE);



static qe_bool is_initialized    = 0;
static qe_ubase main_stack_start = 0;
static qe_uint main_stack_size   = 0;
static qe_ubase text_start       = 0;
static qe_uint text_size         = 0;



/** 
 * @brief Trigger a data abort exception 
 * 
 * @param[in] addr: make a write at this address 
 */
void backtrace_data_abort(qe_ubase addr)
{
    qe_ubase *p = (qe_ubase *)addr;
    *p = 0;
}

typedef void (*abort_fn)(void);

/** 
 * @brief Trigger a prefetch abort exception 
 * 
 * @param[in] addr: make a call at this address 
 */
void backtrace_code_abort(qe_ubase addr)
{
    abort_fn fn = (abort_fn)addr;
    fn();
}

/** 
 * @brief Trigger a div zero exception 
 */
void backtrace_div_abort(void)
{
    double a, b, c;
    a = 10.0;
    b = 0.0;
    c = a / b;
    (void)c;
}

qe_ubase qe_weak qe_main_stack_start(void)
{
    return 0;
}

qe_uint qe_weak qe_main_stack_size(void)
{
    return 0;
}

qe_ubase qe_weak qe_text_start(void)
{
    return 0;
}

qe_uint qe_weak qe_text_size(void)
{
    return 0;
}

qe_ubase qe_weak qe_task_stack_start(void)
{
    return 0;
}

qe_uint qe_weak qe_task_stack_size(void)
{
    return 0;
}

char * qe_weak qe_current_task_name(void)
{
    return QE_NULL;
}

#if defined(__arm__)
/**
 * @brief Get stack frame saved registers
 * 
 * @param[in] fp: frame pointer
 * @param[out] regs: cpu registers struct
 */
void get_frame_regs(qe_ubase fp, qe_backtrace_regs *regs)
{
	/*
	 * Cortext-A push r11 and lr into stack
	 * push {r11, lr} 
	 */
	
    regs->lr = *(qe_ubase*)fp;
    regs->fp = *((qe_ubase*)(fp - 4));
}

/**
 * @brief Get fault saved registers
 * 
 * @param[in] fp: frame pointer
 * @param[out] regs: cpu registers struct
 */
void get_fault_saved_regs(qe_ubase fp, qe_backtrace_regs *regs)
{
	qe_u32 *p;

	/*
	 * see freertos10_xilinx_v1_2/src/port_asm_vectors.S
	 * stmdb	sp!,{r0-r3,r12,lr}
	 *
	 * sp
	 * sp + 4  : r0
	 * sp + 8  : r1
	 * sp + 12 : r2
	 * sp + 16 : r3
	 * sp + 20 : r12
	 * sp + 24 : lr
	 */

	p = (qe_u32 *)fp;

	regs->r0  = p[1];
	regs->r1  = p[2];
	regs->r2  = p[3];
	regs->r3  = p[4];
	regs->r12 = p[5];
	regs->lr  = p[6];
}
#endif

/**
 * @brief Check if fp is in task stack, if true, return task 
 * stack start address and size
 * 
 * @param[in] fp: frame pointer
 * @param[out] stack_start: return task stack start address
 * @param[out] stack_size: return task stack size
 * 
 * @return 
 *  true: is in task stack 
 *  false: not in task stack
 */
static qe_bool is_task_stack(qe_ubase fp, qe_ubase *stack_start, qe_uint *stack_size)
{
    qe_ubase start = qe_task_stack_start();
    qe_uint size  = qe_task_stack_size();
    if (fp >= start && fp <= start + size) {
        *stack_start = start;
        *stack_size = size;
        return qe_true;
    }
    return qe_false;
}

/**
 * @brief Check if fp is in main stack
 * 
 * @param[in] fp: frame pointer
 * 
 * @return 
 *  true: is in main stack
 *  false: not in main stack
 */
static qe_bool is_main_stack(qe_ubase sp)
{
    if (sp >= main_stack_start && sp <= main_stack_start + main_stack_size) {
        return qe_true;
    }
    return qe_false;
}

/**
 * @brief Check if address is in text section
 * 
 * @param[in] addr: address
 * 
 * @return 
 *  true: is in text section
 *  false: not in text section
 */
static qe_bool is_text_section(qe_ubase addr)
{
    if (addr >= text_start && addr <= text_start + text_size) {
        return qe_true;
    }
    return qe_false;
}

/**
 * @brief Backtrace initialize
 * 
 * @param[in] stack_start: main stack start address
 * @param[in] stack_size: main stack size
 * @param[in] code_start: text start address
 * @param[in] code_size: text size
 */
void qe_backtrace_init(qe_ubase stack_start, qe_uint stack_size, 
    qe_ubase code_start, qe_uint code_size)
{
    if (is_initialized) {
        qe_warning("already initialized");
        return;
    }

    main_stack_start = stack_start;
    main_stack_size = stack_size;
    text_start = code_start;
    text_size = code_size;
    is_initialized = 1;

    qe_debug("backtrace init");
    qe_debug("main stack %x %x", main_stack_start, main_stack_size);
    qe_debug("text %x %x", text_start, text_size);
}

/**
 * @brief Get backtrace call stack information
 * 
 * @note This function should using in non-exception handler, like normal
 * running progress or assert.
 * 
 * @param[in] fp: frame pointer
 * @param[out] info: return backtrace information
 */
void qe_backtrace_call_stack(qe_ubase fp, qe_backtrace_info *info)
{
    qe_int i;
    qe_ubase pc, *pos;
    qe_ubase stack_start;
    qe_uint stack_size;
    qe_backtrace_regs regs;

    /* Check if initialized */
    if (!is_initialized) {
        qe_error("not initialized");
        return;
    }

    /* Clear info */
    qe_memset(info, 0, sizeof(qe_backtrace_info));

    info->fp = fp;
    info->is_on_fault = 0;

    /* Check main stack or task stack */
    if (is_task_stack(fp, &stack_start, &stack_size)) {
        info->is_main_stack = 0;
        qe_snprintf(info->taskname, 16, qe_current_task_name());
    } else {
        if (is_main_stack(fp)) {
            info->is_main_stack = 1;
            stack_start = main_stack_start;
            stack_size = main_stack_size;
        } else {
            qe_error("unknown fp %08x", fp);
            return;
        }
    }

    info->stack_start = stack_start;
    info->stack_size = stack_size;

    /* Get stack regs */
    get_frame_regs(fp, &regs);
    info->frame.fp = regs.fp;
    info->frame.lr = regs.lr;
    qe_debug("frame regs fp:%08x lr:%08x", regs.fp, regs.lr);

    /* Copy stack */
    pos = (qe_ubase *)(fp - 8 * sizeof(qe_ubase));
    qe_debug("copy from %p %d", pos, QE_BACKTRACE_STACK_DEPTH);
    for (i=0; i<QE_BACKTRACE_STACK_DEPTH; i++) {
        info->stack[info->stack_depth++] = pos[i];
        qe_debug("stack[%d]:%08x", info->stack_depth-1, 
            info->stack[info->stack_depth-1]);
    }

#if defined(__arm__)
    /* Stack backtrace */
    while (fp > stack_start && fp < (stack_start + stack_size)) {
        pc = *(qe_ubase *)fp;
        fp = *((qe_ubase *)(fp - 4));
        info->calls[info->calls_depth++] = pc;
        qe_debug("calls[%d]:%08x", info->calls_depth-1, 
            info->calls[info->calls_depth-1]);
    }
#endif
}

/**
 * @brief Get backtrace call stack information on fault
 * 
 * @note This function must using on fault handler, like arm exception
 * 
 * @param[in] fp: frame pointer
 * @param[out] info: return backtrace information
 */
void qe_backtrace_call_stack_fault(qe_ubase fp, qe_backtrace_info *info)
{
    qe_int i;
    qe_ubase pc, *pos;
    qe_ubase stack_start;
    qe_uint stack_size;
    qe_backtrace_regs fault_regs;
    qe_backtrace_regs stack_regs;

    /* Check if initialized */
    if (!is_initialized) {
        qe_error("not initialized");
        return;
    }

    /* Check if vaild fp */
    if (!is_main_stack(fp)) {
        qe_error("fp %x not in main stack (%x,%x)", fp,
            main_stack_start, main_stack_start+main_stack_size);
        return;
    }

    /* Clear info */
    qe_memset(info, 0, sizeof(qe_backtrace_info));

    info->is_on_fault = 1;
    info->fp = fp;
    qe_debug("fp %08x", fp);

    /* Get fault saved regs */
    get_fault_saved_regs(fp, &fault_regs);
    qe_debug("fault saved regs:");
    info->fault.r0  = fault_regs.r0;
    info->fault.r1  = fault_regs.r1;
    info->fault.r2  = fault_regs.r2;
    info->fault.r3  = fault_regs.r3;
    info->fault.r12 = fault_regs.r12;
    info->fault.lr  = fault_regs.lr;

#if defined(__arm__)
    qe_debug("  r0 %08x", fault_regs.r0);
    qe_debug("  r1 %08x", fault_regs.r1);
    qe_debug("  r2 %08x", fault_regs.r2);
    qe_debug("  r3 %08x", fault_regs.r3);
    qe_debug("  r12 %08x", fault_regs.r12);
    qe_debug("  lr %08x", fault_regs.lr);
#endif

    /* Get stack regs */
    get_frame_regs(fp, &stack_regs);
    qe_debug("fault frame regs fp:%08x lr:%08x", 
        stack_regs.fp, stack_regs.lr);
    info->frame.fp = stack_regs.fp;
    info->frame.lr = stack_regs.lr;

    /* First record stack lr */
    info->calls[info->calls_depth++] = stack_regs.lr;
    qe_debug("calls[%d]:%x", info->calls_depth-1, 
        info->calls[info->calls_depth-1]);

    /* Second record fault saved lr */
    info->calls[info->calls_depth++] = fault_regs.lr;
    qe_debug("calls[%d]:%x", info->calls_depth-1, 
        info->calls[info->calls_depth-1]);

    /* Jump to prev stack frame */
    fp = stack_regs.fp;

    /* Check which stack */
    if (is_task_stack(fp, &stack_start, &stack_size)) {
        qe_sprintf(info->taskname, "%s", qe_current_task_name());
    } else {
        if (is_main_stack(fp)) {
            info->is_main_stack = 1;
            stack_start = main_stack_start;
            stack_size = main_stack_size;
        } else {
            qe_error("unknown fp %08x", fp);
            return;
        }
    }

    info->stack_start = stack_start;
    info->stack_size = stack_size;

    /* Copy stack */
    pos = (qe_ubase *)(fp - 8 * sizeof(qe_ubase));
    qe_debug("copy from %p %d", pos, QE_BACKTRACE_STACK_DEPTH);
    for (i=0; i<QE_BACKTRACE_STACK_DEPTH; i++) {
        info->stack[info->stack_depth++] = pos[i];
        qe_debug("stack[%d]:%08x", info->stack_depth-1, 
            info->stack[info->stack_depth-1]);
    }

#if defined(__arm__)
    /* Check if stack has next frame and vaild lr */
    for (i=1; i<=8; i++) {
        pc = *((qe_ubase *)(fp - i*sizeof(qe_ubase)));
        if (is_text_section(pc)) {
        	info->calls[info->calls_depth++] = pc;
            qe_debug("calls[%d]:%x", info->calls_depth-1, 
                info->calls[info->calls_depth-1]);
            break;
        }
    }

    /* call stack */
    while (fp > stack_start && fp < (stack_start + stack_size)) {
        pc = *(qe_u32 *)fp;
        qe_debug("pc:%x", pc);
        if (is_text_section(pc)) {
            fp = *((qe_u32 *)(fp - 4));
            info->calls[info->calls_depth++] = pc;
            qe_debug("calls[%d]:%x", info->calls_depth-1, 
                info->calls[info->calls_depth-1]);
        } else {
            fp = pc;
        }
    }
#endif
}

static qe_int info_to_string(qe_backtrace_info *info, 
    char *buffer, int length)
{
    qe_int i;

    if (!buffer || length <=0) {
        qe_error("buffer null");
        return -1;
    }
    
    qe_strb strb = qe_strb_init(buffer, length);

    qe_strb_string(strb, "-------- QELIB Backtrace Start --------\r\n");
    
#if defined(__ARM_ARCH_7A__)
    qe_strb_string(strb, "  arch        : Cortex-A\r\n");
#elif defined(__ARM_ARCH_7M__)
    qe_strb_string(strb, "  arch        : Cortex-M\r\n");
#elif defined(__aarch64__)
    qe_strb_string(strb, "  arch        : AArch64\r\n");
#elif defined(__arm__) || defined(__thumb__)
    qe_strb_string(strb, "  arch        : AArch32\r\n");
#else
    qe_strb_string(strb, "  arch        : unknown\r\n");
#endif

    if (info->is_main_stack) {
    qe_strb_string(strb, "  stack       : main\r\n");
    } else {
    qe_strb_format(strb, "  stack       : task %s\r\n", info->taskname);
    }

    qe_strb_format(strb, "  stack start : %08x\r\n", info->stack_start);
    qe_strb_format(strb, "  stack size  : %08x\r\n", info->stack_size);
    qe_strb_format(strb, "  fp          : %08x\r\n", info->fp);

    qe_strb_format(strb, "  last frame -->\r\n"
                         "    %08x %08x(fp)\r\n"
                         "    %08x %08x(lr)\r\n",
        info->fp - 4, info->frame.fp,
        info->fp, info->frame.lr);

    if (info->is_on_fault) {
    qe_strb_format(strb, "  fault saved -->\r\n"
                         "    %08x %08x(r0)\r\n"
                         "    %08x %08x(r1)\r\n"
                         "    %08x %08x(r2)\r\n"
                         "    %08x %08x(r3)\r\n"
                         "    %08x %08x(r12)\r\n"
                         "    %08x %08x(lr)\r\n",
        info->fp + 4, info->fault.r0,
        info->fp + 8, info->fault.r1,
        info->fp + 12, info->fault.r2,
        info->fp + 16, info->fault.r3,
        info->fp + 20, info->fault.r12,
        info->fp + 24, info->fault.lr);
    }

    if (info->calls_depth) {
        qe_strb_string(strb, "  calls dump -->\r\n");
        for (i=0; i<info->calls_depth; i++) {
            qe_strb_format(strb, "    %08x\r\n", info->calls[i]);
        }
    }

    if (info->stack_depth) {
        qe_strb_string(strb, "  stack dump -->\r\n");
        for (i=0; i<info->stack_depth; i+=4) {
            qe_strb_format(strb, "    %08x %08x %08x %08x\r\n", 
                info->stack[i],
                info->stack[i+1],
                info->stack[i+2],
                info->stack[i+3]);
        }
    }

    qe_strb_string(strb, "-------- QELIB Backtrace End --------\r\n");

    return strb.len;
}

static void info_print(qe_backtrace_info info)
{
    qe_int i;

    qe_printf("\r\n-------- QELIB Backtrace Start --------\r\n");
    
#if defined(__ARM_ARCH_7A__)
    qe_printf("  arch        : Cortex-A\r\n");
#elif defined(__ARM_ARCH_7M__)
    qe_printf("  arch        : Cortex-M\r\n");
#elif defined(__aarch64__)
    qe_printf("  arch        : AArch64\r\n");
#elif defined(__arm__) || defined(__thumb__)
    qe_printf("  arch        : AArch32\r\n");
#else
    qe_printf("  arch        : unknown\r\n");
#endif

    if (info.is_main_stack) {
    qe_printf("  stack       : main\r\n");
    } else {
    qe_printf("  stack       : task %s\r\n", info.taskname);
    }

    qe_printf("  stack start : %08x\r\n", info.stack_start);
    qe_printf("  stack size  : %08x\r\n", info.stack_size);
    qe_printf("  fp          : %08x\r\n", info.fp);

    qe_printf("  last frame -->\r\n"
              "    %08x %08x(fp)\r\n"
              "    %08x %08x(lr)\r\n",
        info.fp - 4, info.frame.fp,
        info.fp, info.frame.lr);

    if (info.is_on_fault) {
#if defined(__arm__)
    qe_printf("  fault saved -->\r\n"
              "    %08x %08x(r0)\r\n"
              "    %08x %08x(r1)\r\n"
              "    %08x %08x(r2)\r\n"
              "    %08x %08x(r3)\r\n"
              "    %08x %08x(r12)\r\n"
              "    %08x %08x(lr)\r\n",
        info.fp + 4, info.fault.r0,
        info.fp + 8, info.fault.r1,
        info.fp + 12, info.fault.r2,
        info.fp + 16, info.fault.r3,
        info.fp + 20, info.fault.r12,
        info.fp + 24, info.fault.lr);
#endif
    }

    if (info.calls_depth) {
        qe_printf("  calls dump -->\r\n");
        for (i=0; i<info.calls_depth; i++) {
            qe_printf("    %08x\r\n", info.calls[i]);
        }
    }

    if (info.stack_depth) {
        qe_printf("  stack dump -->\r\n");
        for (i=0; i<info.stack_depth; i+=4) {
            qe_printf("    %08x %08x %08x %08x\r\n", 
                info.stack[i], 
                info.stack[i+1],
                info.stack[i+2],
                info.stack[i+3]);
        }
    }

    qe_printf("-------- QELIB Backtrace End --------\r\n");
}

/**
 * @brief Get backtrace information into a buffer
 * 
 * @param[in] buffer: string buffer
 * @param[in] length: buffer length
 * 
 * @return string length
 */
qe_int qe_backtrace_get_str(qe_ubase fp, char *buffer, int length)
{
    qe_int n;
    qe_backtrace_info info;

    if (!buffer || length <=0)
        return -1;
    
    qe_backtrace_call_stack(fp, &info);

    return info_to_string(&info, buffer, length);
}

/**
 * @brief Get backtrace fault information into a buffer
 * 
 * @param[in] buffer: string buffer
 * @param[in] length: buffer length
 * 
 * @return string length
 */
qe_int qe_backtrace_get_str_fault(qe_ubase fp, char *buffer, int length)
{
    qe_int n;
    qe_backtrace_info info;

    if (!buffer || length <=0)
        return -1;
    
    qe_backtrace_call_stack_fault(fp, &info);

    return info_to_string(&info, buffer, length);
}

/**
 * @brief Backtrace print
 * 
 * @param[in] fp: frame pointer
 * 
 * @note This function can't using on fault, it will call qe_printf to 
 * print backtrace information
 */
void qe_backtrace_print(qe_ubase fp)
{
    qe_backtrace_info info;
    qe_backtrace_call_stack(fp, &info);
    info_print(info);
}

/**
 * @brief Backtrace print on fault
 * 
 * @param[in] fp: frame pointer
 * 
 * @note This function must using on fault, it will call qe_printf to 
 * print backtrace information
 */
void qe_backtrace_print_fault(qe_ubase fp)
{
    qe_backtrace_info info;

    qe_backtrace_call_stack_fault(fp, &info);

    info_print(info);
}   